/**************************************************************************************************
  Filename:       cent_discovery.c
  Revised:        Laura Kassovic @ MbientLab 
  Revision:       1/21/2014

  Description:    Time App service and characteristic discovery routines.

  Copyright 2011 - 2013 Texas Instruments Incorporated. All rights reserved.

**************************************************************************************************/

/*********************************************************************
 * INCLUDES
 */


#include "Cent.h"

/*********************************************************************
 * MACROS
 */
#define CHAR_DESC_HDL_UUID16_LEN        7
/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */
uint16 centHdlCache[HDL_CACHE_LEN];
/*********************************************************************
 * EXTERNAL VARIABLES
 */


/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */

// Attribute handles used during discovery
static uint16 centSvcStartHdl;
static uint16 centSvcEndHdl;
static uint8 centEndHdlIdx;

/*********************************************************************
 * LOCAL FUNCTIONS
 */
static uint8 centDiscBatt( uint8 state, gattMsgEvent_t *pMsg );
static uint8 centDiscData( uint8 state, gattMsgEvent_t *pMsg );

/*********************************************************************
 * @fn      CentDiscStart()
 *
 * @brief   Start service discovery. 
 *
 *
 * @return  New discovery state.
 */
uint8 centDiscStart( void )
{
  // Clear handle cache
  osal_memset( centHdlCache, 0, sizeof(centHdlCache) );
  
  // Start discovery with first service
  return centDiscGattMsg( DISC_DATA_START, NULL );
}

/*********************************************************************
 * @fn      CentDiscGattMsg()
 *
 * @brief   Handle GATT messages for characteristic discovery. 
 *
 * @param   state - Discovery state.
 * @param   pMsg - GATT message.
 *
 * @return  New discovery state.
 */
uint8 centDiscGattMsg( uint8 state, gattMsgEvent_t *pMsg )
{
  // Execute discovery function for service
  do
  {
    dbg("how the loop works:%x\n",state);
    switch ( state & 0xF0 )
    {
      case DISC_DATA_START:
      {
        state = centDiscData( state , pMsg);

        if ( state == DISC_FAILED || state == DISC_IDLE)
        {
          // dbg("DISC_DATA_START FAILED!\n");
          // state = DISC_IDLE;
        // }else if ( state == DISC_IDLE ){
          dbg("DISC_DATA_START DONE!\n");
          state = DISC_BATT_START;
        }
      }
        break;
      // Alert notification service
      case DISC_BATT_START:
        state = centDiscBatt( state, pMsg );
        if ( state == DISC_FAILED )
        {
          dbg("DISC_BATT_START FAILED!\n");
          state = DISC_IDLE;
        }
        break;
      default:
        break;
    }
  } while ( (state != 0) && ((state & 0x0F) == 0) );
  dbg("who will return :%x\n",state);
  return state;    
}


/*********************************************************************
 * @fn      centDiscData()
 *
 * @brief   Batt notification service and characteristic discovery. 
 *
 * @param   state - Discovery state.
 * @param   pMsg - GATT message.
 *
 * @return  New discovery state.
 */
static uint8 centDiscData( uint8 state, gattMsgEvent_t *pMsg )
{
  uint8 newState = state;
  
  switch ( state )
  {
    case DISC_DATA_START:
      {
        uint8 uuid[ATT_BT_UUID_SIZE] = { LO_UINT16(DATA_SERV_UUID),
                                         HI_UINT16(DATA_SERV_UUID) };

        // Initialize service discovery variables
        centSvcStartHdl = centSvcEndHdl = 0;
        centEndHdlIdx = 0;
        dbg("centConnHandle:%x\n",centConnHandle);
        // Discover service by UUID
        GATT_DiscPrimaryServiceByUUID( centConnHandle, uuid,
                                       ATT_UUID_SIZE, centBLETaskId );      

        newState = DISC_DATA_SVC;
      break;
      }
    case DISC_DATA_SVC:
      {
      dbg("data number: %d, %d \n", pMsg->method , pMsg->msg.findByTypeValueRsp.numInfo );
        // Service found, store handles
      if ( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP &&
           pMsg->msg.findByTypeValueRsp.numInfo > 0 )
      {
        centSvcStartHdl = pMsg->msg.findByTypeValueRsp.handlesInfo[0].handle;
        centSvcEndHdl = pMsg->msg.findByTypeValueRsp.handlesInfo[0].grpEndHandle;
      }
      
      // If procedure complete
      if ( ( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP  && 
             pMsg->hdr.status == bleProcedureComplete ) ||
           ( pMsg->method == ATT_ERROR_RSP ) )
      {
        // If service found
        if ( centSvcStartHdl != 0 )
        {
          // Discover all characteristics
          GATT_DiscAllChars( centConnHandle, centSvcStartHdl,
                             centSvcEndHdl, centBLETaskId );
          dbg(" > here were are?\n");
          newState = DISC_DATA_CHAR;
        }
        else
        {
          dbg(" > centsvcstart == 0?\n");
          // Service not found
          newState = DISC_FAILED;
        }
      }    
      break;
      }
    case DISC_DATA_CHAR:
      {
        uint8   i;
        uint8   *p;
        uint16  handle;
        uint16  uuid;
        
        // Characteristics found
        if ( pMsg->method == ATT_READ_BY_TYPE_RSP &&
             pMsg->msg.readByTypeRsp.numPairs > 0 && 
             pMsg->msg.readByTypeRsp.len == CHAR_DESC_HDL_UUID16_LEN)
        {
          // For each characteristic declaration
          p = pMsg->msg.readByTypeRsp.dataList;
          for ( i = pMsg->msg.readByTypeRsp.numPairs; i > 0; i-- )
          {
            // Parse characteristic declaration
            handle = BUILD_UINT16(p[3], p[4]);
            uuid = BUILD_UINT16(p[5], p[6]);
                   
            // If looking for end handle
            if ( centEndHdlIdx != 0 )
            {
              // End handle is one less than handle of characteristic declaration
              centHdlCache[centEndHdlIdx] = BUILD_UINT16(p[0], p[1]) - 1;
              
              centEndHdlIdx = 0;
            }

            // If UUID is of interest, store handle
            switch ( uuid )
            {
              case DATA_PACK_UUID:
                centHdlCache[HDL_DATA_PACK_START] = handle;
                centEndHdlIdx = HDL_DATA_PACK_END;
                break;
                
              case DATA_ACK_UUID:
                centHdlCache[HDL_DATA_ACK] = handle;
                break;
                
              case DATA_CTRL_UUID:
                centHdlCache[HDL_DATA_CTRL] = handle;
                break;
                
              default:
                break;
            }
            
            p += CHAR_DESC_HDL_UUID16_LEN;
          }
          
        }
          
        // If procedure complete
        if ( ( pMsg->method == ATT_READ_BY_TYPE_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
             ( pMsg->method == ATT_ERROR_RSP ) )
        {
          // Special case of end handle at end of service
          if ( centEndHdlIdx != 0 )
          {
            centHdlCache[centEndHdlIdx] = centSvcEndHdl;
            centEndHdlIdx = 0;
          }
          
          // If didn't find time characteristic
          if ( centHdlCache[HDL_DATA_PACK_START] == 0 )
          {
            dbg("> pack start\n");
            newState = DISC_FAILED;
          }
          else if ( centHdlCache[HDL_DATA_PACK_START] <
                   centHdlCache[HDL_DATA_PACK_END] )
          {
            dbg("centConnHandle = %x ,centBLETaskId = %x\n",centConnHandle,centBLETaskId);
            // Discover characteristic descriptors
            GATT_DiscAllCharDescs( centConnHandle,
                                   centHdlCache[HDL_DATA_PACK_START] + 1,
                                   centHdlCache[HDL_DATA_PACK_END],
                                   centBLETaskId );
                                        
            newState = DISC_DATA_PACK_CCCD;
          }
          else
          {
            newState = DISC_IDLE;
          }
        }
      }      
      break;
    case DISC_DATA_PACK_CCCD:
        {
        uint8 i;
        
        // Characteristic descriptors found
        if ( pMsg->method == ATT_FIND_INFO_RSP &&
             pMsg->msg.findInfoRsp.numInfo > 0 && 
             pMsg->msg.findInfoRsp.format == ATT_HANDLE_BT_UUID_TYPE )
        {
          // For each handle/uuid pair
          for ( i = 0; i < pMsg->msg.findInfoRsp.numInfo; i++ )
          {
            // Look for CCCD
            if ( (pMsg->msg.findInfoRsp.info.btPair[i].uuid[0] ==
                  LO_UINT16(GATT_CLIENT_CHAR_CFG_UUID)) &&
                 (pMsg->msg.findInfoRsp.info.btPair[i].uuid[1] ==
                  HI_UINT16(GATT_CLIENT_CHAR_CFG_UUID)) )
            {
              // CCCD found
              centHdlCache[HDL_DATA_PACK_CCCD] =
                pMsg->msg.findInfoRsp.info.btPair[i].handle;
              
              break;
            }
          }
        }
        
        // If procedure complete
        if ( ( pMsg->method == ATT_FIND_INFO_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
             ( pMsg->method == ATT_ERROR_RSP ) )
        {
          newState = DISC_IDLE;
        }
      }
      break;
    default:
      break;
  }

  return newState;
}


/*********************************************************************
 * @fn      CentDiscBatt()
 *
 * @brief   Batt notification service and characteristic discovery. 
 *
 * @param   state - Discovery state.
 * @param   pMsg - GATT message.
 *
 * @return  New discovery state.
 */
static uint8 centDiscBatt( uint8 state, gattMsgEvent_t *pMsg )
{
  uint8 newState = state;
  
  switch ( state )
  {
    case DISC_BATT_START:  
      {
        uint8 uuid[ATT_BT_UUID_SIZE] = { LO_UINT16(BATT_SERV_UUID),
                                         HI_UINT16(BATT_SERV_UUID) };

        // Initialize service discovery variables
        centSvcStartHdl = centSvcEndHdl = 0;
        centEndHdlIdx = 0;
        dbg("centConnHandle = %x ,centBLETaskId = %x\n",centConnHandle,centBLETaskId);
        // Discover service by UUID
        GATT_DiscPrimaryServiceByUUID( centConnHandle, uuid,
                                       ATT_UUID_SIZE, centBLETaskId );      

        newState = DISC_BATT_SVC;
      } 
      break;

    case DISC_BATT_SVC:
    {
      dbg("batt number: %d, %d \n", pMsg->method , pMsg->msg.findByTypeValueRsp.numInfo );
      // Service found, store handles
      // if ( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP &&
           // pMsg->msg.findByTypeValueRsp.numInfo > 0 )
      {
        centSvcStartHdl = pMsg->msg.findByTypeValueRsp.handlesInfo[0].handle;
        centSvcEndHdl = pMsg->msg.findByTypeValueRsp.handlesInfo[0].grpEndHandle;
      }
      
      // If procedure complete
      if ( ( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP  && 
             pMsg->hdr.status == bleProcedureComplete ) ||
           ( pMsg->method == ATT_ERROR_RSP ) )
      {
        // If service found
        if ( centSvcStartHdl != 0 )
        {
          // Discover all characteristics
          GATT_DiscAllChars( centConnHandle, centSvcStartHdl,
                             centSvcEndHdl, centBLETaskId );
          
          newState = DISC_BATT_CHAR;
        }
        else
        {
          // Service not found
          newState = DISC_FAILED;
        }
      }

    }
      break;
    case DISC_BATT_CHAR:
      {
        uint8   i;
        uint8   *p;
        uint16  handle;
        uint16  uuid;
        dbg("batt number4: %d, %d \n", pMsg->method , pMsg->msg.findByTypeValueRsp.numInfo );
        // Characteristics found
        if ( pMsg->method == ATT_READ_BY_TYPE_RSP &&
             pMsg->msg.readByTypeRsp.numPairs > 0 && 
             pMsg->msg.readByTypeRsp.len == CHAR_DESC_HDL_UUID16_LEN )
             // pMsg->msg.readByTypeRsp.len == CHAR_DESC_HDL_UUID128_LEN )
        {
          // For each characteristic declaration
          p = pMsg->msg.readByTypeRsp.dataList;
          for ( i = pMsg->msg.readByTypeRsp.numPairs; i > 0; i-- )
          {
            // Parse characteristic declaration
            handle = BUILD_UINT16(p[3], p[4]);
            uuid = BUILD_UINT16(p[5], p[6]);

            // If looking for end handle
            if ( centEndHdlIdx != 0 )
            {
              // End handle is one less than handle of characteristic declaration
              centHdlCache[centEndHdlIdx] = BUILD_UINT16(p[0], p[1]) - 1;
              centEndHdlIdx = 0;
            }

            // If UUID is of interest, store handle
            switch ( uuid )
            {
              case BATT_LEVEL_UUID:
                centHdlCache[HDL_BATT_LEVEL_START] = handle;
                centEndHdlIdx = HDL_BATT_LEVEL_END;
                break;

              default:
                break;
            }
            
            // p += CHAR_DESC_HDL_UUID128_LEN;
            p += CHAR_DESC_HDL_UUID16_LEN;
          }
          
        }
        
        // If procedure complete
        if ( ( pMsg->method == ATT_READ_BY_TYPE_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
             ( pMsg->method == ATT_ERROR_RSP ) )
        {
          // Special case of end handle at end of service
          if ( centEndHdlIdx != 0 )
          {
            centHdlCache[centEndHdlIdx] = centSvcEndHdl;
            centEndHdlIdx = 0;
          }
          // If didn't find mandatory characteristic
          if ( centHdlCache[HDL_BATT_LEVEL_START] == 0 )
          {
            newState = DISC_FAILED;
          }
          else if ( centHdlCache[HDL_BATT_LEVEL_START] <
                    centHdlCache[HDL_BATT_LEVEL_END] )
          {
            // Discover characteristic descriptors
            GATT_DiscAllCharDescs( centConnHandle,
                                   centHdlCache[HDL_BATT_LEVEL_START] + 1,
                                   centHdlCache[HDL_BATT_LEVEL_END],
                                   centBLETaskId );
                                        
            newState = DISC_BATT_LVL_CCCD;
          }
          else
          {
            newState = DISC_IDLE;
          }
        }
      }      
      break;
      
      case DISC_BATT_LVL_CCCD:
      {
        uint8 i;
        dbg("batt number3: %d, %d \n", pMsg->method , pMsg->msg.findByTypeValueRsp.numInfo );
        // Characteristic descriptors found
        if ( pMsg->method == ATT_FIND_INFO_RSP &&
             pMsg->msg.findInfoRsp.numInfo > 0 && 
             pMsg->msg.findInfoRsp.format == ATT_HANDLE_BT_UUID_TYPE )
        {
          // For each handle/uuid pair
          for ( i = 0; i < pMsg->msg.findInfoRsp.numInfo; i++ )
          {
            // Look for CCCD
            if ( (pMsg->msg.findInfoRsp.info.btPair[i].uuid[0] ==
                  LO_UINT16(GATT_CLIENT_CHAR_CFG_UUID)) &&
                 (pMsg->msg.findInfoRsp.info.btPair[i].uuid[1] ==
                  HI_UINT16(GATT_CLIENT_CHAR_CFG_UUID)) )
            {
              // CCCD found
              centHdlCache[HDL_BATT_LEVEL_CCCD] =
                pMsg->msg.findInfoRsp.info.btPair[i].handle;
              
              break;
            }
          }
        }
        // If procedure complete
        if ( ( pMsg->method == ATT_FIND_INFO_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
             ( pMsg->method == ATT_ERROR_RSP ) )
        {
          newState = DISC_IDLE;
        }
      }
      break;

    default:
      break;
  }
  
  return newState;
}

/*********************************************************************
*********************************************************************/